/**
 * Based on http://shazsterblog.blogspot.com/2011/11/arduino-google-voice-activated-servo.html
 * We control this servo motor from Lift
 */

#include <Servo.h>
#define MAX 150
#define MIN 0

int greenLedPin = 2;
int redLedPin = 3;
int servoPin = 9;
int servoPin2 = 10;
int legSpeed = 5;
int headSpeed = 5;
int limits[2] = {
  0,150};
byte command = '0';
boolean open = false;

Servo legJoint;
Servo headJoint;

void setup() {
  Serial.begin(9600);
  legJoint.attach(servoPin);
  headJoint.attach(servoPin2);
  legJoint.write((limits[1]-limits[0])/2);
  headJoint.write((limits[1]-limits[0])/2);
  pinMode(greenLedPin, OUTPUT);
  pinMode(redLedPin, OUTPUT);
}

void loop() {
  if(Serial.available() >= 2) {
   //if(Serial.read() == 0xff) {
    command = Serial.read();
    int legAngle = legJoint.read();
    int headAngle = headJoint.read();
    Serial.print("Leg Angle: ");
    Serial.println(legAngle);
    Serial.print("Head Angle: ");
    Serial.println(headAngle);
    Serial.println(legSpeed);    
    if (legAngle >= limits[1] || legAngle <= limits[0])  legSpeed = -legSpeed;
    if (headAngle >= limits[1] || headAngle <= limits[0])  headSpeed = -headSpeed;
    if(command == '1' && legAngle <= limits[1] && legSpeed > 0) {
      legJoint.write(legAngle + legSpeed);
    } 
    else if(command == '2' && legAngle >= limits[0] && legSpeed > 0) {
      legJoint.write(legAngle - legSpeed);
    }
    if(command == '3' & headAngle <= limits[1] && headSpeed > 0) {
      headJoint.write(headAngle + headSpeed);
    } 
    else if(command == '4' & headAngle >= limits[0] && headSpeed > 0) {
      headJoint.write(headAngle - headSpeed);
    }
    else if(command == '5') {
      digitalWrite(redLedPin, LOW);
      digitalWrite(greenLedPin, HIGH);
    }
    else if(command == '6') {
      digitalWrite(greenLedPin, LOW);
      digitalWrite(redLedPin, HIGH);
    }
    else if(command == '7') {
      digitalWrite(greenLedPin, LOW);
      digitalWrite(redLedPin, LOW);
    } 
   //}  
  }  
  delay(10);
}










